/*
 * File:   Box2dEngine.cpp
 * Author: Elevator
 *
 * Created on 24 Май 2013 г., 22:47
 */

#include "Box2dEngine.h"
//#include <GL/gl.h>
#include "Bodies/Box2dBody.h"

#include "Joints/Box2dHingeJoint.h"
#include "Joints/Box2dDistanceJoint.h"
#include "Joints/Box2dParallelJoint.h"

#include "Shapes/Box2dShape.h"
#include "Shapes/Box2dCircleShape.h"
#include "Shapes/Box2dPolygonShape.h"

#include <Box2D/Box2D.h>
#include <stdio.h>

namespace physics {

	b2BodyType BodyTypeToB2Type( IBody::Type value );
	IBody::Type B2TypeToBodyType( b2BodyType value );

	Box2dEngine::Box2dEngine( Vec2 gravity ) {
		int flags = 0;
		flags += 1 * b2DebugDraw::e_shapeBit;
		flags += 1 * b2DebugDraw::e_jointBit;
		flags += 0 * b2DebugDraw::e_aabbBit;
		flags += 0 * b2DebugDraw::e_pairBit;
		flags += 1 * b2DebugDraw::e_centerOfMassBit;

		_world = new b2World( b2Vec2( gravity.X, gravity.Y ), true );
	}

	Box2dEngine::~Box2dEngine( ) {
		delete _world;
	}

	IHingeJoint* Box2dEngine::CreateHingeJoint(
		IBody* bodyA, IBody* bodyB,
		const Vec2& anchor,
		bool collide,
		bool enableLimit, float leftLimit, float rightLimit,
		bool enableMotor, float motorForce, float motorSpeed ) {

		//	Box2dBody* bA = (Box2dBody*) bodyA;
		//	Box2dBody* bB = (Box2dBody*) bodyB;

		Box2dBody* bA = dynamic_cast<Box2dBody*> ( bodyA );
		Box2dBody* bB = dynamic_cast<Box2dBody*> ( bodyB );

		b2RevoluteJointDef jointDef;
		jointDef.Initialize( bA->GetBox2dBody( ), bB->GetBox2dBody( ), b2Vec2( anchor.X, anchor.Y ) );
		jointDef.collideConnected = collide;
		jointDef.enableLimit = enableLimit;
		jointDef.lowerAngle = rightLimit;
		jointDef.upperAngle = leftLimit;

		jointDef.enableMotor = enableMotor;
		jointDef.motorSpeed = motorSpeed;
		jointDef.maxMotorTorque = motorForce;

		//	b2RevoluteJoint* joint = (b2RevoluteJoint*) _world->CreateJoint( &jointDef );
		b2RevoluteJoint* joint = dynamic_cast<b2RevoluteJoint*> ( _world->CreateJoint( &jointDef ) );

		return new Box2dHingeJoint( bA, bB, joint );
	}

	void Box2dEngine::DestroyHingeJoint( IHingeJoint* joint ) {
		//Box2dHingeJoint* b2joint = (Box2dHingeJoint*) joint;
		Box2dHingeJoint* b2joint = dynamic_cast<Box2dHingeJoint*> ( joint );

		_world->DestroyJoint( b2joint->GetBox2dJoint( ) );
		delete joint;
	}

	IDistanceJoint* Box2dEngine::CreateDistanceJoint(
		IBody* bodyA, IBody* bodyB,
		const Vec2& anchorA, const Vec2& anchorB,
		bool collide,
		float length, float frequency,
		float dampingRatio ) {

		//	Box2dBody* bA = (Box2dBody*) bodyA;
		//	Box2dBody* bB = (Box2dBody*) bodyB;

		Box2dBody* bA = dynamic_cast<Box2dBody*> ( bodyA );
		Box2dBody* bB = dynamic_cast<Box2dBody*> ( bodyB );

		b2DistanceJointDef jointDef;
		jointDef.Initialize( bA->GetBox2dBody( ), bB->GetBox2dBody( ), b2Vec2( anchorA.X, anchorA.Y ), b2Vec2( anchorB.X, anchorB.Y ) );
		jointDef.collideConnected = collide;
		jointDef.length = length;
		jointDef.frequencyHz = frequency;
		jointDef.dampingRatio = dampingRatio;

		//	b2DistanceJoint* joint = (b2DistanceJoint*) _world->CreateJoint( &jointDef );
		b2DistanceJoint* joint = dynamic_cast<b2DistanceJoint*> ( _world->CreateJoint( &jointDef ) );
		return new Box2dDistanceJoint( bA, bB, joint );
	}

	void Box2dEngine::DestroyDistanceJoint( IDistanceJoint* joint ) {
		//	Box2dDistanceJoint* b2joint = (Box2dDistanceJoint*) joint;
		Box2dDistanceJoint* b2joint = dynamic_cast<Box2dDistanceJoint*> ( joint );
		_world->DestroyJoint( b2joint->GetBox2dJoint( ) );
		delete joint;
	}

	IParallelJoint* Box2dEngine::CreateParallelJoint(
		IBody* bodyA, IBody* bodyB,
		const Vec2& anchor, const Vec2& axis,
		bool collide,
		bool enableLimit, float lowerLimit, float upperLimit,
		bool enableMotor, float motorForce, float motorSpeed ) {

		//	Box2dBody* bA = (Box2dBody*) bodyA;
		//	Box2dBody* bB = (Box2dBody*) bodyB;

		Box2dBody* bA = dynamic_cast<Box2dBody*> ( bodyA );
		Box2dBody* bB = dynamic_cast<Box2dBody*> ( bodyB );

		b2PrismaticJointDef jointDef;
		jointDef.Initialize( bA->GetBox2dBody( ), bB->GetBox2dBody( ), b2Vec2( anchor.X, anchor.Y ), b2Vec2( axis.X, axis.Y ) );
		jointDef.collideConnected = collide;
		jointDef.enableLimit = enableLimit;
		jointDef.lowerTranslation = lowerLimit;
		jointDef.upperTranslation = upperLimit;
		jointDef.enableMotor = enableMotor;
		jointDef.maxMotorForce = motorForce;
		jointDef.motorSpeed = motorSpeed;

		//b2PrismaticJoint* joint = (b2PrismaticJoint*) _world->CreateJoint( &jointDef );
		b2PrismaticJoint* joint = dynamic_cast<b2PrismaticJoint*> ( _world->CreateJoint( &jointDef ) );
		return new Box2dParallelJoint( bA, bB, joint );
	}

	void Box2dEngine::DestroyParallelJoint( IParallelJoint* joint ) {
		//Box2dParallelJoint* b2joint = (Box2dParallelJoint*) joint;
		Box2dParallelJoint* b2joint = dynamic_cast<Box2dParallelJoint*> ( joint );

		_world->DestroyJoint( b2joint->GetBox2dJoint( ) );
		delete joint;
	}

	IBody* Box2dEngine::CreateBody( IBody::Type type,
		const Transform& position,
		const Transform& velocity,
		float mass, float inertia ) {

		b2BodyDef bodyDef;
		bodyDef.position = b2Vec2( position.Vec.X, position.Vec.Y );
		bodyDef.angle = position.Rot;
		bodyDef.linearVelocity = b2Vec2( velocity.Vec.X, velocity.Vec.Y );
		bodyDef.angularVelocity = velocity.Rot;
		bodyDef.type = BodyTypeToB2Type( type );

		b2MassData massData;
		massData.I = inertia;
		massData.mass = mass;
		massData.center = b2Vec2(0.0f, 0.0f);

		b2Body* body = _world->CreateBody( &bodyDef );
		body->SetMassData( &massData );

		return new Box2dBody( body );
	}

	void Box2dEngine::DestroyBody( IBody* body ) {
		//	Box2dBody* bB = (Box2dBody*) bodyB;
		Box2dBody* b = dynamic_cast<Box2dBody*> ( body );

		_world->DestroyBody( b->GetBox2dBody( ) );
	}

	ICircleShape* Box2dEngine::CreateCircleShape( Vec2 pos, float radius ) {
		Box2dCircleShape* shape = new Box2dCircleShape( new b2CircleShape( ) );
		shape->SetPos( Transform( pos, 0.0f ) );
		shape->SetRadius( radius );
		return shape;
	}

	IPolygonShape* Box2dEngine::CreatePolygonShape( Transform pos ) {
		b2PolygonShape* b2shape = new b2PolygonShape( );
		Box2dPolygonShape* shape = new Box2dPolygonShape( pos, b2shape );
		return shape;
	}

	void Box2dEngine::Update( float dt ) {
		_world->Step( dt, 5, 5 );
	}

	b2BodyType BodyTypeToB2Type( IBody::Type value ) {
		switch( value ) {
			case IBody::Dynamic: return b2_dynamicBody;
			case IBody::Static: return b2_staticBody;
			case IBody::Kinematic: return b2_kinematicBody;
			default: return b2_staticBody;
		}
	}

	IBody::Type B2TypeToBodyType( b2BodyType value ) {
		switch( value ) {
			case b2_dynamicBody: return IBody::Dynamic;
			case b2_staticBody: return IBody::Static;
			case b2_kinematicBody: return IBody::Kinematic;
			default: return IBody::Static;
		}
	}
}
